#include "query_handler.h"
#include <malloc.h>
#include <values.h>
#include <stdio.h>
#include <assert.h>
#include <string.h>

#include <gsl/gsl_vector.h>
#include <gsl/gsl_sf_log.h>
#include <gsl/gsl_sf_pow_int.h>
#include <gsl/gsl_blas.h>


void* filterAnd(void * input, struct filter_state * s) {
  void * ret = input;
  int i;
  for(i = 0; ret && i < s->count; i++) {
    ret = s->functions[i](ret, s->states[i]);
  }
  return ret;
}

struct knn_state *  knn_state_alloc(int k, void * queryObject, double(*distance)(const void *, const void *), size_t result_size) {
  struct knn_state * s = malloc(sizeof(struct knn_state));
  s->queryObject = queryObject;
  s->result_size = result_size;
  s->results = malloc (k * s->result_size);
  s->results_distance = malloc (k * sizeof(double));
  s->distance = (double(*)(const void * , const void *))distance;
  s->k = k;
  knn_state_reset(s);
  return s;
}

void knn_state_free(struct knn_state * s) {
  free(s->results);
  free(s->results_distance);
  free(s);
}

void knn_state_print(struct knn_state * s, void(*printObj)(void*, void*)) {
  int i;
  printf("knn_state: [");
  printf("  query_object: ");
  printObj(s->queryObject, 0);
  for(i = 0 ; i < s->k; i++) {
    if(s->results_distance[i] == MAXDOUBLE) {
      printf("-\n");
    } else {
      printf("  (%f)  ", s->results_distance[i]);
      printObj(s->results + (i * s->result_size), 0);
    }
  }
  printf("  k=%d]\n", s->k);
}

void knn_state_reset(struct knn_state * s) {
  int i;

  s->max_dist = -1;
  s->max_pos  = 0;
  


  for(i = 0; i < s->k; i++) {
    //    s->results[i] = 0;
    s->results_distance[i] = MAXDOUBLE;
  }
}

void* knn(void * result, struct knn_state * s) {
  double this_distance = (*s->distance)(s->queryObject, result);
  //  xyzi_print((struct xyzi*)result, 0);


  if(1||s->max_dist < 0) {
    int i;

    for (i = 0; i < s->k; i++) {
      if(s->results_distance[i] > s->max_dist) {
	s->max_dist = s->results_distance[i];
	s->max_pos  = i;
      }
    }
  }

  if(this_distance < s->max_dist) {
    //   printf("pos=%d, dist=%f\n", s->max_pos, s->max_dist);
    assert(s->max_pos >= 0);
    assert(s->max_pos < s->k);
    
    s->results_distance[s->max_pos] = this_distance;
    //    s->results[s->max_pos] = result;
    
    memcpy(s->results + (s->max_pos * s->result_size), result, s->result_size);

    s->max_dist = -1;
    s->max_pos  = 0;

    return result;
  } else {
    return 0;
  }
}


void* count_hits(void * hit, off_t* hit_count) {
  *hit_count = 1 + *hit_count;
  return hit;
}



void dstate_alloc(struct dstate* d, size_t rSize, size_t pSize, size_t qSize){
  d->r0 = malloc(rSize);
  d->r1 = malloc(rSize);
  d->qSize = qSize;
  d->n = 0;
  d->results = 0;
  d->p0 = malloc(pSize);
}

void dstate_add_result(struct dstate* d, void* p){
  if(d->results == 0){
    d->results = malloc(d->qSize);
  } else {
    d->results = realloc(d->results, d->n*d->qSize);
  }
  memcpy((char *)(d->results)+d->n*d->qSize, p, d->qSize);
  d->n++;
}


void* filter_weak_cond(void* p, const void* state){
  struct dstate* cond = (struct dstate*)state;
  if( (cond->in(cond->p0, cond->d(cond->p0, p), cond->r0) > 0) ||
      (cond->in(cond->p0, cond->d(cond->p0, p), cond->r1) > 0)){
    return(p);
  } else {
    return(0);
  }
}


void* filter_strong_cond(void* p, const void* state){
  struct dstate* cond = (struct dstate*)state;
  if( (cond->in(cond->p0, cond->d(cond->p0, p), cond->r0) > 0) &&
      (cond->in(cond->p0, cond->d(cond->p0, p), cond->r1) > 0)){
    return(p);
  } else {
    return(0);
  }
}


double radius_test(void* p0, double dp, const void* r){
  if(dp <= ((struct dpoint*)(r))->dist){
    return(dp);
  } else {
    return(-1);
  }
}

const void* store_add(const void* obj, struct store* const s){

  int i=0;
  void* o;

  i = store_new(s);
  o = store_read(s, i);
  memcpy(o, obj, store_blockSize(s));
  store_write(s,i);
  store_release(s,i);
  return obj;
}

const void* print_with_distance(const void* obj, struct print_with_distance_state* s) {

  printf("%0.3f\t", s->distance(obj, s->query_object));
  s->print(s->query_object, s->print_state1);
  printf("\t");
  s->print(obj         , s->print_state2);
  printf("\n");
  return obj;
}
